#include "predict_by_acc.hpp"

namespace sros::location
{
    predict_by_acc::predict_by_acc()
        : predict_impl()
    {
        init_();
    }

    void predict_by_acc::init_()
    {
        // 状态迁移矩阵
        param_->F_ << 1, param_->dt_, 0, 
                      0, 1, 0, 
                      0, 0, 0;

        // 控制输入矩阵
        param_->B_ << 0.5*param_->dt_*param_->dt_, param_->dt_, 1;

        // 初始化状态
        param_->X_.setZero();

        // 协方差矩阵
        param_->P_.setIdentity();

        // 过程噪声矩阵
        param_->Q_ << 0.05, 0, 0,
                      0, 0.1, 0,
                      0, 0, 0.5;

        // 控制输入置零
        param_->U_  = 0.0f;
    }
}